#include "aos/kernel.h"
#include <aos/mbmaster.h>
#include "modbus.h"
#include "aos/cli.h"

#define MODBUS_TASK_DEPTH 1024

#define MOTO_ETA_ID  (5)

#define CTRL_REG     (0x2000)
#define OUT_HZ       (0x2103)
#define OUT_A        (0x2104)

#define ETA_OPEN_VAL     (0x12)
#define ETA_CLOSE_VAL    (0x11)
#define ETA_POS_VAL      ETA_OPEN_VAL
#define ETA_NAG_VAL      (0x22)

#define eta_open         set_reg(MOTO_ETA_ID, CTRL_REG, ETA_OPEN_VAL)
#define eta_nag        set_reg(MOTO_ETA_ID, CTRL_REG, ETA_NAG_VAL)
#define eta_close        set_reg(MOTO_ETA_ID, CTRL_REG, ETA_CLOSE_VAL)
#define eta_get_a        (get_reg(MOTO_ETA_ID, OUT_A)/100.0)
#define eta_get_hz       (get_reg(MOTO_ETA_ID, OUT_HZ)/100.0)
#define eta_set_hz(val)  set_reg(MOTO_ETA_ID, OUT_HZ, val*100)

static int eta_flag = 0;
float eta_aa = 0.0f;
int eta_aa_open = 0;
extern int roof_flag;
void ctrl_eta(int val)
{
	eta_flag = val;
}

void eta_opt(int val)
{
	switch(val){
		case 0:
			eta_open;
			eta_aa_open = 1;
			break;
		case 1:
			eta_nag;
			break;
		case 2:
			eta_close;
			eta_aa_open = 0;
			break;
		default:
			break;
	}
}

static void eta_cmd_routine(char *buf, int len, int argc, char **argv)
{
	int ret = 0;
	int arg1 = strtoul(argv[2], NULL, 10);
	
	switch (*(argv[1]))
	{
	case 'o':
	   eta_open;
	   eta_aa_open = 1;
	   aos_cli_printf("open eta\r\n");
	   break;
	case 'n':
		eta_nag;
	   aos_cli_printf("stop eta\r\n");
		break;
	case 'c':
		eta_close;
		eta_aa_open = 0;
	   aos_cli_printf("stop eta\r\n");
	   break;
	case 'v':
		eta_set_hz(arg1);
	   aos_cli_printf("stop eta\r\n");
	   break;
	case 'g':
	   aos_cli_printf("eta A:%d\r\n", eta_get_a);
	   break;
	case 'p':
		ctrl_eta(arg1);
	   break;

	default:
	   aos_cli_printf("eta Unknown cs %d\r\n", *(argv[1]));
	   break;
	}
}


static struct cli_command eta_cmd[] = {
    { "eta","control modbusr", eta_cmd_routine },
};

static int adjust_end = 0;
int adjust(float aa)
{
	if (aa - 1.3 > 0 && adjust_end != 1){
		upping(1);
		adjust_end = 1;
	}else if (aa - 1.2 < 0 && adjust_end != 2){
		upping(0);
		adjust_end = 2;
	}else{
		if (adjust_end){
			stop_up();
			adjust_end = 0;
		}
	}
}

void eta_engine()
{
	float eta_temp = 0.0;
    while (1) {
		if (eta_aa_open){
			//eta_temp = eta_get_a;
			if (eta_temp > 0.5){
				eta_aa = eta_temp;
			}
			if (roof_flag){
				adjust(eta_aa);
			}
		}
		if (eta_flag){
			printf("eta A %f\r\n", eta_aa);
		}
		aos_msleep(300);
    }
}

void eta_init()
{
	int32_t rc = -1;
	rc = aos_task_new("moto_eta", eta_engine, NULL, MODBUS_TASK_DEPTH);
    if (0 != rc) {
		printf("moto eta creat fail\r\n");
	}
	aos_cli_register_commands(&eta_cmd[0], sizeof(eta_cmd) / sizeof(struct cli_command));
}



